
// This Arduino example demonstrates bidirectional operation of a 
// 28BYJ-48, using a ULN2003 interface board to drive the stepper.
// The 28BYJ-48 motor is a 4-phase, 8-beat motor, geared down by
// a factor of 68. One bipolar winding is on motor pins 1 & 3 and
// the other on motor pins 2 & 4. The step angle is 5.625/64 and the 
// operating Frequency is 100pps. Current draw is 92mA. 
////////////////////////////////////////////////

#define CLOCKWISE 1
#define ANTICLOCKWISE 2
#define QUEUE_SIZE 100

struct MOTOR {
  byte m1Pin;
  byte m2Pin;
  byte m3Pin;
  byte m4Pin;
};

MOTOR RMotor = { 10, 11, 12, 13 };
MOTOR LMotor = { 2, 3, 4, 5 };
MOTOR FMotor = { 6, 7, 8, 9 };
MOTOR BMotor = { 22, 24, 26, 28 };
MOTOR UMotor = { 21, 23, 25, 27 };
MOTOR DMotor = { 30, 32, 34, 36 };

int motorSpeed = 1200;  //variable to set stepper speed
int count = 0;          // count of steps made
int countsperrev = 512; // number of steps per full revolution
int countsperturn = countsperrev >> 2;
int lookup[8] = {B01000, B01100, B00100, B00110, B00010, B00011, B00001, B01001};

char sequenceQueue[QUEUE_SIZE];
byte currIndex = 0;
byte currSize = 0;

void setup() {
  setPins(RMotor.m1Pin, RMotor.m2Pin, RMotor.m3Pin, RMotor.m4Pin);
  setPins(LMotor.m1Pin, LMotor.m2Pin, LMotor.m3Pin, LMotor.m4Pin);
  setPins(FMotor.m1Pin, FMotor.m2Pin, FMotor.m3Pin, FMotor.m4Pin);
  setPins(BMotor.m1Pin, BMotor.m2Pin, BMotor.m3Pin, BMotor.m4Pin);
  setPins(UMotor.m1Pin, UMotor.m2Pin, UMotor.m3Pin, UMotor.m4Pin);
  setPins(DMotor.m1Pin, DMotor.m2Pin, DMotor.m3Pin, DMotor.m4Pin);
  Serial.begin(9600);
}

void setPins(byte motorPin1, byte motorPin2, byte motorPin3, byte motorPin4) {
  pinMode(motorPin1, OUTPUT);
  pinMode(motorPin2, OUTPUT);
  pinMode(motorPin3, OUTPUT);
  pinMode(motorPin4, OUTPUT);
}

void loop() {
  if (currSize != currIndex) {
    rotate(popChar());
  } else {
    fillQueue();
  }
}

void fillQueue() {
  if (Serial.available()) {
    int av = Serial.available();
    for (int i=0;i<av;i++) {
      char c = Serial.read();
      sequenceQueue[(currSize++)%QUEUE_SIZE] = c;
    }
  }
}

char popChar() {
  char c = (currIndex == currSize) ? -1 : sequenceQueue[currIndex++ % QUEUE_SIZE];
  return c;
}

void rotate(char c) {
  MOTOR m = RMotor;
  byte dir = c >= 'a' ? CLOCKWISE : ANTICLOCKWISE;
  switch(c) {
    case 'r':
    case 'R':
      m = RMotor;
      break;
    case 'l':
    case 'L':
      m = LMotor;
      break;
    case 'f':
    case 'F':
      m = FMotor;
      break;
    case 'b':
    case 'B':
      m = BMotor;
      break;
    case 'u':
    case 'U':
      m = UMotor;
      break;
    case 'd':
    case 'D':
      m = DMotor;
      break;
    default:
      dir = 0;
  }
  switch (dir) {
    case CLOCKWISE:
      for (int i=0;i<countsperturn;i++) {
        clockwise(m.m1Pin, m.m2Pin, m.m3Pin, m.m4Pin);
      }
      break;
    case ANTICLOCKWISE:
      for (int i=0;i<countsperturn;i++) {
        anticlockwise(m.m1Pin, m.m2Pin, m.m3Pin, m.m4Pin);
      }
      break;
  }
}

//////////////////////////////////////////////////////////////////////////////
//set pins to ULN2003 high in sequence from 1 to 4
//delay "motorSpeed" between each pin setting (to determine speed)
void anticlockwise(byte motorPin1, byte motorPin2, byte motorPin3, byte motorPin4) {
  for(int i = 0; i < 8; i++) {
    setOutput(motorPin1, motorPin2, motorPin3, motorPin4, i);
    delayMicroseconds(motorSpeed);
  }
}

void clockwise(byte motorPin1, byte motorPin2, byte motorPin3, byte motorPin4) {
  for(int i = 7; i >= 0; i--) {
    setOutput(motorPin1, motorPin2, motorPin3, motorPin4, i);
    delayMicroseconds(motorSpeed);
  }
}

void setOutput(byte motorPin1, byte motorPin2, byte motorPin3, byte motorPin4, int out) {
  digitalWrite(motorPin1, bitRead(lookup[out], 0));
  digitalWrite(motorPin2, bitRead(lookup[out], 1));
  digitalWrite(motorPin3, bitRead(lookup[out], 2));
  digitalWrite(motorPin4, bitRead(lookup[out], 3));
}

